#include <sys/time.h>
#include <stdint.h>
#include <stdio.h>
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
#include <string.h>
#include <errno.h>
#include <stdlib.h>
#include "gps.h"

void gps_open_session(speed_t baud_rate, gps_session_t *session)
{
	// open port
	int serial_fd;
	if ((serial_fd = open("/dev/ttyS2", O_RDWR | O_NOCTTY | O_NONBLOCK)) < 0) {
		// failed to open port, return invalid file descriptor immediately
		session->fd = serial_fd;
		return;
	}

	// configure port

	struct termios mode;
	tcgetattr(serial_fd, &mode);

	// set baud rate
	cfsetispeed(&mode, baud_rate);
	cfsetospeed(&mode, baud_rate);

	mode.c_cflag |= (CLOCAL | CREAD);

	// 8 bits, no parity, 1 stop bit, no flow control
	mode.c_cflag &= ~(PARENB | CSTOPB | CSIZE);
	mode.c_cflag |= CS8;
	mode.c_cflag &= ~CRTSCTS;

	mode.c_iflag = mode.c_oflag = mode.c_lflag = (tcflag_t)0;

	tcsetattr(serial_fd, TCSANOW, &mode);

	// enable blocking mode
	int flags = fcntl(serial_fd, F_GETFL, 0);
	flags &= ~O_NONBLOCK;
	fcntl(serial_fd, F_SETFL, flags);

	session->fd = serial_fd;
	session->rx_count = 0;
}

void gps_close_session(gps_session_t *session)
{
	close(session->fd);
	session->rx_count = 0;
}

// Reads as much available serial data as possible and appends it to the buffer.
void gps_fill_buffer(gps_session_t *session)
{
	if (session->rx_count < GPS_BUFFER_SIZE) {
		int count;
		if ((count = read(session->fd, session->rx_buffer + session->rx_count, GPS_BUFFER_SIZE - session->rx_count)) < 0) {
			return;
		}
		session->rx_count += count;
	}
}

// Shifts the buffer contents to the left by the specified amount.
void gps_shift_buffer(gps_session_t *session, int shift)
{
	int left = 0, right = shift;
	while (right < session->rx_count) {
		session->rx_buffer[left] = session->rx_buffer[right];
		left++; right++;
	}
	session->rx_count -= shift;
}

int gps_read_sentence(gps_session_t *session, char *buf, int len)
{
	// prime the buffer
	if (session->rx_count == 0)
		gps_fill_buffer(session);

	// find opening $
	int start = -1;
	while (start < 0) {
		for (int i = 0; i < session->rx_count; i++) {
			if (session->rx_buffer[i] == '$') {
				start = i + 1;
				break;
			}
		}
		if (start < 0) {
			// no $ yet, read in more data
			// we can ignore previous data since it is not part of a valid sentence
			session->rx_count = 0;
			gps_fill_buffer(session);
		}
	}
	// shift buffer contents so that the start of the sentence is at index 0
	gps_shift_buffer(session, start);

	// find closing \r
	int length = -1;
	while(length < 0) {
		for (int i = 0; i < session->rx_count; i++) {
			if (session->rx_buffer[i] == '\r') {
				length = i;
				break;
			}
		}
		if (length < 0) {
			// no \r yet, read in more data
			if (session->rx_count > len || session->rx_count == GPS_BUFFER_SIZE) {
				// sentence is too long already, ignore all buffered data
				session->rx_count = 0;
				return -1;
			}
			gps_fill_buffer(session);
		}
	}

	if (length > len) {
		// sentence is too long
		// remove sentence from buffer
		gps_shift_buffer(session, length + 2);
		return -1;
	} else {
		// copy sentence (without delimiters) into buf
		memcpy(buf, session->rx_buffer, length);
		buf[length] = '\0'; // add null terminator
	}

	// remove sentence from buffer
	gps_shift_buffer(session, length + 2);

	return length;
}

const fixed_t FIXED_60  = { 0x00000003C0000000LL };
const fixed_t FIXED_180 = { 0x0000000B40000000LL };

fixed_t gps_deg_min_to_rad(int deg, fixed_t min)
{
	fixed_t res = { ((int64_t)deg) << FIXED_FRAC_BITS };
	res.val += fixed_div(min, FIXED_60).val;
	res = fixed_div(fixed_mul(res, FIXED_PI), FIXED_180);
	return res;
}

int gps_get_field(const char *sentence, int field, char *buf, int len)
{
	int length;
	const char *start, *end;
	const char *ptr = sentence;

	// find start
	while (field > 0) {
		while (*ptr != '\0' && *ptr != ',') ptr++;
		if (*ptr == ',') {
			ptr++;
			field--;
		} else {
			// reached end of string before reaching field
			return -1;
		}
	}
	start = ptr;

	// find end
	while (*ptr != '\0' && *ptr != ',') ptr++;
	end = ptr;

	length = end - start;
	if (length > len) {
		return -1;
	}
	memcpy(buf, start, length);
	buf[length] = '\0';

	return length;
}

#define GPS_MAX_FIELD_SIZE 15

int gps_parse_gpgga(const char *sentence, gps_data_t *data);
int gps_parse_pgrmv(const char *sentence, gps_data_t *data);

int gps_parse_sentence(const char *sentence, gps_data_t *data)
{
	if (strncmp(sentence, "GPGGA", 5) == 0) {
		return gps_parse_gpgga(sentence, data);
	} else if (strncmp(sentence, "PGRMV", 5) == 0) {
		return gps_parse_pgrmv(sentence, data);
	}
}

int gps_parse_gpgga(const char *sentence, gps_data_t *data)
{
	char field[GPS_MAX_FIELD_SIZE + 1];
	char deg[4];
	deg[3] = '\0';

	// latitude: field 2 (dddmm.mmmmm)
	if (gps_get_field(sentence, 2, field, GPS_MAX_FIELD_SIZE) < 5) return GPS_INFO_NONE;
	strncpy(deg, field, 3);
	data->coord.lat = gps_deg_min_to_rad(atoi(deg), fixed_from_string(field + 3));

	// N/S: field 3, N positive, S negative
	if (gps_get_field(sentence, 3, field, GPS_MAX_FIELD_SIZE) != 1) return GPS_INFO_NONE;
	if (field[0] == 'S') data->coord.lat.val = -data->coord.lat.val;

	// longitude: field 4 (dddmm.mmmm)
	if (gps_get_field(sentence, 4, field, GPS_MAX_FIELD_SIZE) < 5) return GPS_INFO_NONE;
	strncpy(deg, field, 3);
	data->coord.lon = gps_deg_min_to_rad(atoi(deg), fixed_from_string(field + 3));

	// E/W: field 5, E positive, W negative
	if (gps_get_field(sentence, 5, field, GPS_MAX_FIELD_SIZE) != 1) return GPS_INFO_NONE;
	if (field[0] == 'W') data->coord.lon.val = -data->coord.lon.val;

	// quality: field 6
	if (gps_get_field(sentence, 6, field, GPS_MAX_FIELD_SIZE) != 1) return GPS_INFO_NONE;
	data->quality = atoi(field);

	// altitude: field 9
	if (gps_get_field(sentence, 9, field, GPS_MAX_FIELD_SIZE) < 1) return GPS_INFO_NONE;
	data->coord.alt = fixed_from_string(field);

	data->info |= GPS_INFO_POSITION;
	return GPS_INFO_POSITION;
}


int gps_parse_pgrmv(const char *sentence, gps_data_t *data)
{
	char field[GPS_MAX_FIELD_SIZE + 1];

	// vx: field 1
	if (gps_get_field(sentence, 1, field, GPS_MAX_FIELD_SIZE) < 1) return GPS_INFO_NONE;
	data->velocity.x = fixed_from_string(field);
	// vy: field 2
	if (gps_get_field(sentence, 2, field, GPS_MAX_FIELD_SIZE) < 1) return GPS_INFO_NONE;
	data->velocity.y = fixed_from_string(field);
	// vz: field 3
	if (gps_get_field(sentence, 3, field, GPS_MAX_FIELD_SIZE) < 1) return GPS_INFO_NONE;
	data->velocity.z = fixed_from_string(field);

	data->info |= GPS_INFO_VELOCITY;
	return GPS_INFO_VELOCITY;
}

const fixed_t WGS84_A          = { 0x0006152990000000LL }; // a: earth semi-major axis
const fixed_t WGS84_E2         = { 0x00000000001b6b91LL }; // e^2: first eccentricity squared
const fixed_t WGS84_1_MINUS_E2 = { 0x000000000fe4946fLL }; // 1 - e^2

void gps_create_ref_frame(const gps_coord_t *geo, ref_frame_t *ref)
{
	// copy geodetic coordinate
	ref->geodetic.lat.val = geo->lat.val;
	ref->geodetic.lon.val = geo->lon.val;
	ref->geodetic.alt.val = geo->alt.val;

	// compute sines and cosines
	fixed_t sin_lat, cos_lat;
	fixed_t sin_lon, cos_lon;
	fixed_sin_cos(geo->lat, &sin_lat, &cos_lat);
	fixed_sin_cos(geo->lon, &sin_lon, &cos_lon);

	// compute ECEF coordinate (gps_geodetic_to_ecef is not used to avoid computing the sines and cosines twice)
	fixed_t c = fixed_sqrt(fixed_sub(FIXED_ONE, fixed_mul(WGS84_E2, fixed_mul(sin_lat, sin_lat))));
	fixed_t a_over_c = fixed_div(WGS84_A, c);
	fixed_t temp = fixed_mul(fixed_add(a_over_c, geo->alt), cos_lat);
	ref->ecef.x = fixed_mul(temp, cos_lon);
	ref->ecef.y = fixed_mul(temp, sin_lon);
	ref->ecef.z = fixed_mul(fixed_add(fixed_mul(a_over_c, WGS84_1_MINUS_E2), geo->alt), sin_lat);

	// compute transformation matrix

	ref->ecef_to_enu.m11.val = -sin_lon.val;
	ref->ecef_to_enu.m12 = cos_lon;
	ref->ecef_to_enu.m13 = FIXED_ZERO;

	ref->ecef_to_enu.m21.val = -fixed_mul(sin_lat, cos_lon).val;
	ref->ecef_to_enu.m22.val = -fixed_mul(sin_lat, sin_lon).val;
	ref->ecef_to_enu.m23 = cos_lat;

	ref->ecef_to_enu.m31 = fixed_mul(cos_lat, cos_lon);
	ref->ecef_to_enu.m32 = fixed_mul(cos_lat, sin_lon);
	ref->ecef_to_enu.m33 = sin_lat;
}

void gps_geodetic_to_ecef(const gps_coord_t *geo, vector3_t *ecef)
{
	// compute sines and cosines
	fixed_t sin_lat, cos_lat;
	fixed_t sin_lon, cos_lon;
	fixed_sin_cos(geo->lat, &sin_lat, &cos_lat);
	fixed_sin_cos(geo->lon, &sin_lon, &cos_lon);

	fixed_t c = fixed_sqrt(fixed_sub(FIXED_ONE, fixed_mul(WGS84_E2, fixed_mul(sin_lat, sin_lat))));
	fixed_t a_over_c = fixed_div(WGS84_A, c);
	fixed_t temp = fixed_mul(fixed_add(a_over_c, geo->alt), cos_lat);
	ecef->y = fixed_mul(temp, sin_lon);
	ecef->x = fixed_mul(temp, cos_lon);
	ecef->z = fixed_mul(fixed_add(fixed_mul(a_over_c, WGS84_1_MINUS_E2), geo->alt), sin_lat);
}

void gps_geodetic_to_enu(const ref_frame_t *ref, const gps_coord_t *geo, vector3_t *enu)
{
	vector3_t temp;
	gps_geodetic_to_ecef(geo, &temp);
	temp.x.val -= ref->ecef.x.val;
	temp.y.val -= ref->ecef.y.val;
	temp.z.val -= ref->ecef.z.val;
	vector3_transform(&ref->ecef_to_enu, &temp, enu);
}
